//Holder for vectors, vertices, or RGB colors (I should have just used a color class, but it's too late now)

//Has exclusive use of global buffer vectors bufferC and bufferD
final class Vector3d{
  public float x;
  public float y; 
  public float z;
  Vector3d(float myX, float myY, float myZ){
//    debugCounter++;
    x = myX; y = myY; z = myZ;
  }
  
  Vector3d(Vector3d _vec){
//    debugCounter++;
    x = _vec.x;
    y = _vec.y;
    z = _vec.z;
  }
  
  public Vector3d copy(){
//    debugCounter++;
    return new Vector3d(x,y,z);
  }
  
  public Vector3d plus(Vector3d addMe){
//    debugCounter++;
    return new Vector3d(x+addMe.x,y+addMe.y,z+addMe.z);
  }
  
  public void set(float _x, float _y, float _z){
    x=_x; y=_y; z=_z;
  }
  
  public void set(Vector3d vec){
    x=vec.x; y=vec.y; z=vec.z;
  }
  
  public void add(Vector3d addMe){
    x += addMe.x; y += addMe.y; z += addMe.z;
  }
  
  public void subtract(Vector3d subMe){
    x -= subMe.x; y -= subMe.y; z -= subMe.z;
  }
  
  public void multiply(float multMe){
    x *= multMe;
    y *= multMe;
    z *= multMe;
  }
  
  public Vector3d minus(Vector3d addMe){
//        debugCounter++;
    return new Vector3d(x-addMe.x,y-addMe.y,z-addMe.z);
  }

  public Vector3d times(float multMe){
//    debugCounter++;
    return new Vector3d(x*multMe,y*multMe,z*multMe);
  }
  
  public float norm(){
    return sqrt(x*x+y*y+z*z);
  }
  
  public float normSquared(){
    return (x*x+y*y+z*z);
  }
  
  public float dot(Vector3d dotMe){
    return (x*dotMe.x+y*dotMe.y+z*dotMe.z);
  }
  
  public void normalize(){
//    debugCounter++;
    float tnorm = this.norm();
    if (tnorm < .0001) {
      //println("Zero norm");
      //Screw it, we can't do anything reasonable here
      //Maybe we should make this a boolean function, return false if we get here?
      return;
    }
    float myNormI = 1.0/tnorm;
    x *= myNormI;
    y *= myNormI;
    z *= myNormI;
  }
  
  public Vector3d normalized(){
//    debugCounter++;
    Vector3d result = new Vector3d(this);
    result.normalize();
    return result;
  }
  
  public Vector3d direction(){
//    debugCounter++;
    float myNorm = this.norm();
    return new Vector3d(x/myNorm,y/myNorm,z/myNorm);
  }
  
  public float distanceTo(Vector3d _vec){
    return sqrt( (x-_vec.x)*(x-_vec.x)+(y-_vec.y)*(y-_vec.y)+(z-_vec.z)*(z-_vec.z) );
  }
  
  public float distanceSqr(Vector3d _vec){
    return (x-_vec.x)*(x-_vec.x)+(y-_vec.y)*(y-_vec.y)+(z-_vec.z)*(z-_vec.z);
  }
  
  public Vector3d directionTo(Vector3d _vec){
//    debugCounter++;
    Vector3d result = new Vector3d( _vec.x-x,_vec.y-y,_vec.z-z);
    result.normalize();
    return result;
  }
  
  public Vector3d cross(Vector3d crossMe){
//    debugCounter++;
    return new Vector3d( (y*crossMe.z-z*crossMe.y), (z*crossMe.x-x*crossMe.z),  (x*crossMe.y-y*crossMe.x) );
  }
  
  public Vector3d projectAlong(Vector3d alongMe){
    Vector3d buffer = alongMe.normalized();
    buffer.multiply(this.dot(buffer));
    return buffer;
  }
  
  /*
  To inline projectAlong:
  float invMyNorm = 1.0/alongMe.norm();
  float bx = alongMe.x * invMyNorm;
  float by = alongMe.y * invMyNorm;
  float thisdotb = this.x*bx+this.y*by;
  float resultX = bx*thisdotb;
  float resultY = by*thisdotb;
  */
  
  
  public Vector3d reflectOver(Vector3d lineStart, Vector3d lineEnd){
    /*Vector3d diff = lineEnd.minus(lineStart);
    Vector3d diff2 = lineEnd.minus(this);
    Vector3d proj = diff2.projectAlong(diff);
    Vector3d perp = diff2.minus(proj);
    return this.plus(perp.times(2.0));*/
    return reflectOver(lineStart,lineEnd,1.0);
  }
  
  public Vector3d reflectOver(Vector3d lineStart, Vector3d lineEnd, float perpCoef){
    //Same as above, but scales perpendicular distance by perpCoef (1 is a perfect reflection, 0 moves it to line, -1 doesn't move it at all) - useful for collisions
    Vector3d diff = lineEnd.minus(lineStart);
    Vector3d diff2 = lineEnd.minus(this);
    Vector3d proj = diff2.projectAlong(diff);
    Vector3d perp = diff2.minus(proj);
    return this.plus(perp.times(1.0+perpCoef));
    
    //Version with temp Vector3d creation eliminated - disabled until can be tested
/*    bufferC.set(lineEnd); bufferC.subtract(lineStart);
    bufferD.set(lineEnd); bufferD.subtract(this);
    float invMyNorm = 1.0/bufferC.norm();
    float bx = bufferC.x * invMyNorm;
    float by = bufferC.y * invMyNorm;
    float thisdotb = bufferD.x*bx+bufferD.y*by;
    bufferC.set(bx*thisdotb,by*thisdotb,0.0);
    bufferD.subtract(bufferC);
    bufferD.multiply(1.0+perpCoef);
    return this.plus(bufferD);    */
  }
  
  public float distanceToLine(Vector3d lineStart, Vector3d lineEnd){
    Vector3d diff = lineEnd.minus(lineStart);
    Vector3d diff2 = lineEnd.minus(this);
    Vector3d proj = diff2.projectAlong(diff);
    Vector3d perp = diff2.minus(proj);
    return perp.norm();
    
/*    //Version with temp Vector3d creation eliminated - disabled until can be tested
    bufferC.set(lineEnd); bufferC.subtract(lineStart);
    bufferD.set(lineEnd); bufferD.subtract(this);
    float invMyNorm = 1.0/bufferC.norm();
    float bx = bufferC.x * invMyNorm;
    float by = bufferC.y * invMyNorm;
    float thisdotb = bufferD.x*bx+bufferD.y*by;
    bufferC.set(bx*thisdotb,by*thisdotb,0.0);
    bufferD.subtract(bufferC);
    return bufferD.norm();    */
  }
  
  public Vector3d displacementToLine(Vector3d lineStart, Vector3d lineEnd){
    /*Vector3d diff = lineEnd.minus(lineStart);
    Vector3d diff2 = lineEnd.minus(this);
    Vector3d proj = diff2.projectAlong(diff);
    return diff2.minus(proj);*/
    
    //Version with temp Vector3d creation eliminated
    bufferC.set(lineEnd); bufferC.subtract(lineStart);
    bufferD.set(lineEnd); bufferD.subtract(this);
    float cnorm = bufferC.norm();
    if (cnorm < .0001) { return new Vector3d(0,0,0); }
    float invMyNorm = 1.0/cnorm;
    float bx = bufferC.x * invMyNorm;
    float by = bufferC.y * invMyNorm;
    float thisdotb = bufferD.x*bx+bufferD.y*by;
    bufferC.set(bx*thisdotb,by*thisdotb,0.0);
    bufferD.subtract(bufferC);
//    debugCounter++;
    return new Vector3d(bufferD);
  }
  
  public void displacementToLineSet(Vector3d lineStart, Vector3d lineEnd, Vector3d result){
    //Same as displacementToLine, but this version just loads the result into result instead of creating new vector
    bufferC.set(lineEnd); bufferC.subtract(lineStart);
    bufferD.set(lineEnd); bufferD.subtract(this);
    float cnorm = bufferC.norm();
    if (cnorm < .0001) {result.set(0,0,0); return;}
    float invMyNorm = 1.0/cnorm;
    float bx = bufferC.x * invMyNorm;
    float by = bufferC.y * invMyNorm;
    float thisdotb = bufferD.x*bx+bufferD.y*by;
    bufferC.set(bx*thisdotb,by*thisdotb,0.0);
    bufferD.subtract(bufferC);
//    debugCounter++;
    result.set(bufferD);
  }

}//end of Vector3d class
